#include<ros/ros.h>
#include <atomic>

#include<geometry_msgs/PoseStamped.h>
#include<geometry_msgs/TwistStamped.h>
#include<std_msgs/UInt64.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include<sensor_msgs/PointCloud2.h>
#include<std_msgs/Int32MultiArray.h>

#include "unionsys_core.hpp"

#include"../SDPltUtils/SDPltBase.hpp"
#include "../SDPltDDS/SDPltPublisher.hpp"
#include "../SDPltDDS/SDPltSubscriber.hpp"

#include "../SDPltDDSPose/SDPltPoseMsg.h"
#include "../SDPltDDSPose/SDPltPoseMsgPubSubTypes.h"
#include "../SDPltDDSPose/SDPltVelocityMsg.h"
#include "../SDPltDDSPose/SDPltVelocityMsgPubSubTypes.h"
#include "../SDPltDDSPose/SDPltStateMsg.h"
#include "../SDPltDDSPose/SDPltStateMsgPubSubTypes.h"
#include "../SDPltDDSPose/pointcloud2.h"
#include "../SDPltDDSPose/pointcloud2PubSubTypes.h"

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/common/common.h>
#include <pcl/filters/voxel_grid.h>

#include <octomap/octomap.h>
#include <octomap_msgs/Octomap.h>
#include <octomap_msgs/conversions.h>
#include <visualization_msgs/Marker.h>

#include<iostream>
#include<fstream>
#include <string>


//接收dds消息，并发布到ros节点
using namespace std;

RegisterMSN *reg_sub = new RegisterMSN();

struct Box
{
	float x_min;
	float y_min;
	float z_min;
	float x_max;
	float y_max;
	float z_max;
	int id;
};

struct Point3D {
    float x, y, z, i;
 
    // 构造函数，方便初始化
    Point3D(float x = 0, float y = 0, float z = 0, float i = 0) : x(x), y(y), z(z), i(i) {}
};

pcl::PointCloud<pcl::PointXYZ>  cloud_fusion;
sensor_msgs::PointCloud2 t_rosCloud;

double sample_size = 0.2;
int drone_id_cnt = 0;
std::vector<geometry_msgs::PoseStamped> human_list;
std::vector<geometry_msgs::PoseStamped> car_list;
std::vector<geometry_msgs::PoseStamped> oil_list;
std::vector<geometry_msgs::PoseStamped> weapon_list;
std::vector<geometry_msgs::PoseStamped> bigarmor_list;
std::vector<geometry_msgs::PoseStamped> shatanche_list;
std::vector<geometry_msgs::PoseStamped> drone_pose_vector(22);

std::vector<Point3D> door_position_list = {
    {4.5, -5.1, 0.5, 0},
    {13.6, -15.0, 0.5, 0}, 
    {13.6, -17.5, 0.5, 0},
    {13.6, -20.7, 0.5, 0},
    {13.6, -23.1, 0.5, 0},
    {15.4, -30.2, 0.5, 0},
    {20.0, -30.2, 0.5, 0},
    {16.6, -35.5, 0.5, 0},
    //floor 2
    {13.6, -12.5, 4.0, 0},
    {13.6, -24.7, 4.0, 0},
    {13.6, -33.5, 4.0, 0}
};

ros::Subscriber rossub_pose, rossub_target, rossub_pointcloud, rossub_planner;

//声明fastdds订阅、ros的发布类
ros::Publisher rospub_poseall;
ros::Publisher rospub_pose1; 
ros::Publisher rospub_pose2; 
ros::Publisher rospub_pose3; 
ros::Publisher rospub_pose4; 
ros::Publisher rospub_pose5; 
ros::Publisher rospub_pose6; 
ros::Publisher rospub_pose7; 
ros::Publisher rospub_pose8; 
ros::Publisher rospub_pose9; 
ros::Publisher rospub_pose10; 
ros::Publisher rospub_pose11; 
ros::Publisher rospub_pose12; 
ros::Publisher rospub_pose13; 
ros::Publisher rospub_pose14; 
ros::Publisher rospub_pose15; 
ros::Publisher rospub_pose16; 
ros::Publisher rospub_pose17; 
ros::Publisher rospub_pose18; 
ros::Publisher rospub_pose19; 
ros::Publisher rospub_pose20; 
ros::Publisher rospub_pose21; 
ros::Publisher rospub_pose22; 


ros::Publisher drone_state_pub, marker_pub, marker_id_pub, drone_id_pub;
ros::Publisher human_point_pub, car_point_pub, oil_point_pub, weapon_point_pub;
ros::Publisher rospub_pointcloud, rospub_planner; 

void goal_callback(const geometry_msgs::PoseStamped::ConstPtr &goal_msg);

void planner_callback(const visualization_msgs::Marker::ConstPtr &target_msg);


void target_callback(const geometry_msgs::PoseStamped::ConstPtr &target_msg);

void pointcloud_callback(const sensor_msgs::PointCloud2::ConstPtr &drone_cloud);

void draw_box(Box temp_box, int id, double width, double red, double green, double blue);

void draw_name(geometry_msgs::PoseStamped draw_pose,std::string name, int id);


void draw_drone_name(geometry_msgs::PoseStamped draw_pose,std::string name, int id);


void draw_waypoints(std::vector<geometry_msgs::Pose> draw_poses, int id,double width);


//fastdds(pose)回调函数

void cloud_pub_callback(const ros::TimerEvent& /*event*/)
{

    //publish cloud
    if (cloud_fusion.points.size() != cloud_fusion.width * cloud_fusion.height) {
        std::cout<<"====>error points222.."<<std::endl;
    }
    cloud_fusion.width = cloud_fusion.points.size();
    cloud_fusion.height = 1;
    
    pcl::toROSMsg(cloud_fusion,t_rosCloud);
    t_rosCloud.header.frame_id = "map";
    rospub_pointcloud.publish(t_rosCloud);
    // if(cloud_fusion.points.size()>0)
    // {
    //     pcl::io::savePCDFileASCII ("/home/unionsys/swarm_ws/src/wlcb_dimianzhan/pose_file/map.pcd", cloud_fusion);//保存pcd
    // }
    

}
void human_draw_callback(const ros::TimerEvent& /*event*/)
{
    // std::ofstream outfile("/home/unionsys/swarm_ws/src/wlcb_dimianzhan/pose_file/human_coord.txt",std::ios_base::out);
    // if (!outfile.is_open())
	// {
	// 	std::cout << "can not open this file" << std::endl;
	// 	return;
	// }
    int clusterId = 0;
    for(int i=0;i<human_list.size();i++)
    {
        pcl::PointXYZ pt;
        pt.x = human_list[i].pose.position.x;
        pt.y = human_list[i].pose.position.y;
        pt.z = human_list[i].pose.position.z;
        Box box;
        box.x_min = pt.x-0.5;
        box.y_min = pt.y-0.5;
        box.z_min = pt.z-0.5;
        box.x_max = pt.x+0.5;
        box.y_max = pt.y+0.5;
        box.z_max = pt.z+0.5;
        box.id = clusterId;
        draw_box(box, clusterId, 0.2, 1, 0, 0);
        geometry_msgs::PoseStamped draw_name_temppose;
        draw_name_temppose.pose.position.x = pt.x;
        draw_name_temppose.pose.position.y = pt.y;
        draw_name_temppose.pose.position.z = pt.z+1.0;
        draw_name(draw_name_temppose,"human", clusterId); 
        // outfile<<fixed<<setprecision(2)<<pt.x<<" "<<pt.y<<" "<<pt.z<<std::endl;
        clusterId++;
    }
    // outfile.close();
    
}
void car_draw_callback(const ros::TimerEvent& /*event*/)
{
    // std::ofstream outfile("/home/unionsys/swarm_ws/src/wlcb_dimianzhan/pose_file/car_coord.txt",std::ios_base::out);
    // if (!outfile.is_open())
	// {
	// 	std::cout << "can not open this file" << std::endl;
	// 	return;
	// }
    int clusterId = 100;
    for(int i=0;i<car_list.size();i++)
    {
        pcl::PointXYZ pt;
        pt.x = car_list[i].pose.position.x;
        pt.y = car_list[i].pose.position.y;
        pt.z = car_list[i].pose.position.z;
        Box box;
        box.x_min = pt.x-0.5;
        box.y_min = pt.y-0.5;
        box.z_min = pt.z-0.5;
        box.x_max = pt.x+0.5;
        box.y_max = pt.y+0.5;
        box.z_max = pt.z+0.5;
        box.id = clusterId;
        draw_box(box, clusterId, 0.2, 1, 0, 0);
        geometry_msgs::PoseStamped draw_name_temppose;
        draw_name_temppose.pose.position.x = pt.x;
        draw_name_temppose.pose.position.y = pt.y;
        draw_name_temppose.pose.position.z = pt.z+1.0;
        draw_name(draw_name_temppose,"car", clusterId); 
        // outfile<<fixed<<setprecision(2)<<pt.x<<" "<<pt.y<<" "<<pt.z<<std::endl;
        clusterId++;
    }
    // outfile.close();
}
void oil_draw_callback(const ros::TimerEvent& /*event*/)
{
    // std::ofstream outfile("/home/unionsys/swarm_ws/src/wlcb_dimianzhan/pose_file/oil_coord.txt",std::ios_base::out);
    // if (!outfile.is_open())
	// {
	// 	std::cout << "can not open this file" << std::endl;
	// 	return;
	// }
    int clusterId = 200;
    for(int i=0;i<oil_list.size();i++)
    {
        pcl::PointXYZ pt;
        pt.x = oil_list[i].pose.position.x;
        pt.y = oil_list[i].pose.position.y;
        pt.z = oil_list[i].pose.position.z;
        Box box;
        box.x_min = pt.x-0.5;
        box.y_min = pt.y-0.5;
        box.z_min = pt.z-0.5;
        box.x_max = pt.x+0.5;
        box.y_max = pt.y+0.5;
        box.z_max = pt.z+0.5;
        box.id = clusterId;
        draw_box(box, clusterId, 0.2, 1, 0, 0);
        geometry_msgs::PoseStamped draw_name_temppose;
        draw_name_temppose.pose.position.x = pt.x;
        draw_name_temppose.pose.position.y = pt.y;
        draw_name_temppose.pose.position.z = pt.z+1.0;
        draw_name(draw_name_temppose,"oil", clusterId); 
        // outfile<<fixed<<setprecision(2)<<pt.x<<" "<<pt.y<<" "<<pt.z<<std::endl;
        clusterId++;
    }
    // outfile.close();
}
void weapon_draw_callback(const ros::TimerEvent& /*event*/)
{
    // std::ofstream outfile("/home/unionsys/swarm_ws/src/wlcb_dimianzhan/pose_file/weapon_coord.txt",std::ios_base::out);
    // if (!outfile.is_open())
	// {
	// 	std::cout << "can not open this file" << std::endl;
	// 	return;
	// }
    int clusterId = 300;
    for(int i=0;i<weapon_list.size();i++)
    {
        pcl::PointXYZ pt;
        pt.x = weapon_list[i].pose.position.x;
        pt.y = weapon_list[i].pose.position.y;
        pt.z = weapon_list[i].pose.position.z;
        Box box;
        box.x_min = pt.x-0.5;
        box.y_min = pt.y-0.5;
        box.z_min = pt.z-0.5;
        box.x_max = pt.x+0.5;
        box.y_max = pt.y+0.5;
        box.z_max = pt.z+0.5;
        box.id = clusterId;
        draw_box(box, clusterId, 0.2, 1, 0, 0);
        geometry_msgs::PoseStamped draw_name_temppose;
        draw_name_temppose.pose.position.x = pt.x;
        draw_name_temppose.pose.position.y = pt.y;
        draw_name_temppose.pose.position.z = pt.z+1.0;
        draw_name(draw_name_temppose,"weapon", clusterId); 
        // outfile<<fixed<<setprecision(2)<<pt.x<<" "<<pt.y<<" "<<pt.z<<std::endl;
        clusterId++;
    }
    // outfile.close();
}
void bigarmor_draw_callback(const ros::TimerEvent& /*event*/)
{
    // std::ofstream outfile("/home/unionsys/swarm_ws/src/wlcb_dimianzhan/pose_file/bigarmor_coord.txt",std::ios_base::out);
    // if (!outfile.is_open())
	// {
	// 	std::cout << "can not open this file" << std::endl;
	// 	return;
	// }
    int clusterId = 400;
    for(int i=0;i<bigarmor_list.size();i++)
    {
        pcl::PointXYZ pt;
        pt.x = bigarmor_list[i].pose.position.x;
        pt.y = bigarmor_list[i].pose.position.y;
        pt.z = bigarmor_list[i].pose.position.z;
        Box box;
        box.x_min = pt.x-0.5;
        box.y_min = pt.y-0.5;
        box.z_min = pt.z-0.5;
        box.x_max = pt.x+0.5;
        box.y_max = pt.y+0.5;
        box.z_max = pt.z+0.5;
        box.id = clusterId;
        draw_box(box, clusterId, 0.2, 1, 0, 0);
        geometry_msgs::PoseStamped draw_name_temppose;
        draw_name_temppose.pose.position.x = pt.x;
        draw_name_temppose.pose.position.y = pt.y;
        draw_name_temppose.pose.position.z = pt.z+1.0;
        draw_name(draw_name_temppose,"bigarmor", clusterId); 
        // outfile<<fixed<<setprecision(2)<<pt.x<<" "<<pt.y<<" "<<pt.z<<std::endl;
        clusterId++;
    }
    // outfile.close();
}
void shatanche_draw_callback(const ros::TimerEvent& /*event*/)
{
    // std::ofstream outfile("/home/unionsys/swarm_ws/src/wlcb_dimianzhan/pose_file/shatanche_coord.txt",std::ios_base::out);
    // if (!outfile.is_open())
	// {
	// 	std::cout << "can not open this file" << std::endl;
	// 	return;
	// }
    int clusterId = 500;
    for(int i=0;i<shatanche_list.size();i++)
    {
        pcl::PointXYZ pt;
        pt.x = shatanche_list[i].pose.position.x;
        pt.y = shatanche_list[i].pose.position.y;
        pt.z = shatanche_list[i].pose.position.z;
        Box box;
        box.x_min = pt.x-0.5;
        box.y_min = pt.y-0.5;
        box.z_min = pt.z-0.5;
        box.x_max = pt.x+0.5;
        box.y_max = pt.y+0.5;
        box.z_max = pt.z+0.5;
        box.id = clusterId;
        draw_box(box, clusterId, 0.2, 1, 0, 0);
        geometry_msgs::PoseStamped draw_name_temppose;
        draw_name_temppose.pose.position.x = pt.x;
        draw_name_temppose.pose.position.y = pt.y;
        draw_name_temppose.pose.position.z = pt.z+1.0;
        draw_name(draw_name_temppose,"shatanche", clusterId); 
        // outfile<<fixed<<setprecision(2)<<pt.x<<" "<<pt.y<<" "<<pt.z<<std::endl;
        clusterId++;
    }
    // outfile.close();
}
void door_draw_callback(const ros::TimerEvent& /*event*/)
{

    int clusterId = 600;
    for(int i=0;i<door_position_list.size();i++)
    {
        if(door_position_list[i].i == 0)
        {
            for(int j=0;j<drone_pose_vector.size();j++)
            {
                double dist_to_vechie = sqrt((door_position_list[i].x-drone_pose_vector[j].pose.position.x)*(door_position_list[i].x-drone_pose_vector[j].pose.position.x)
                                            +(door_position_list[i].y-drone_pose_vector[j].pose.position.y)*(door_position_list[i].y-drone_pose_vector[j].pose.position.y)
                                            +(door_position_list[i].z-drone_pose_vector[j].pose.position.z)*(door_position_list[i].z-drone_pose_vector[j].pose.position.z));
                if(dist_to_vechie<5)
                {
                    door_position_list[i].i = 1;
                    break;
                }
            }
        }
        if(door_position_list[i].i == 1)
        {
            Box box;
            box.x_min = door_position_list[i].x-0.5;
            box.y_min = door_position_list[i].y-0.5;
            box.z_min = door_position_list[i].z-0.5;
            box.x_max = door_position_list[i].x+0.5;
            box.y_max = door_position_list[i].y+0.5;
            box.z_max = door_position_list[i].z+0.5;
            box.id = clusterId;
            draw_box(box, clusterId, 0.2, 1, 0, 0);
            geometry_msgs::PoseStamped draw_name_temppose;
            draw_name_temppose.pose.position.x = door_position_list[i].x;
            draw_name_temppose.pose.position.y = door_position_list[i].y;
            draw_name_temppose.pose.position.z = door_position_list[i].z+1.0;
            draw_name(draw_name_temppose,"door", clusterId); 
            clusterId++;
        }
        
    }
}



int main(int argc, char* argv[]){
    reg_sub->detect_serial_number();
    ros::init(argc, argv, "SDPltDDSPosesub");
    ros::NodeHandle nodeHandle; 
    // 设置循环的频率
    ros::Rate loop_rate(500);


    ros::Timer cloud_pub_timer = nodeHandle.createTimer(ros::Duration(0.2), cloud_pub_callback);
    ros::Timer human_draw_timer = nodeHandle.createTimer(ros::Duration(1.0), human_draw_callback);
    ros::Timer car_draw_timer = nodeHandle.createTimer(ros::Duration(1.0), car_draw_callback);
    ros::Timer oil_draw_timer = nodeHandle.createTimer(ros::Duration(1.0), oil_draw_callback);
    ros::Timer weapon_draw_timer = nodeHandle.createTimer(ros::Duration(1.0), weapon_draw_callback);
    ros::Timer bigarmor_draw_timer = nodeHandle.createTimer(ros::Duration(1.0), bigarmor_draw_callback);
    ros::Timer shatanche_draw_timer = nodeHandle.createTimer(ros::Duration(1.0), shatanche_draw_callback);
    ros::Timer door_draw_timer = nodeHandle.createTimer(ros::Duration(1.0), door_draw_callback);

    
    //dds先接收topic，后ros再发布topic
    
    rossub_pose = nodeHandle.subscribe<geometry_msgs::PoseStamped>("/zhao/pose_all_drone", 10, goal_callback);

    rospub_poseall = nodeHandle.advertise<geometry_msgs::PoseStamped>("/zhao/all_drone",100);

    rospub_pose1 = nodeHandle.advertise<geometry_msgs::PoseStamped>("/drone1/local_position/pose",10);
    rospub_pose2 = nodeHandle.advertise<geometry_msgs::PoseStamped>("/drone2/local_position/pose",10);
    rospub_pose3 = nodeHandle.advertise<geometry_msgs::PoseStamped>("/drone3/local_position/pose",10);
    rospub_pose4 = nodeHandle.advertise<geometry_msgs::PoseStamped>("/drone4/local_position/pose",10);
    rospub_pose5 = nodeHandle.advertise<geometry_msgs::PoseStamped>("/drone5/local_position/pose",10);
    rospub_pose6 = nodeHandle.advertise<geometry_msgs::PoseStamped>("/drone6/local_position/pose",10);
    rospub_pose7 = nodeHandle.advertise<geometry_msgs::PoseStamped>("/drone7/local_position/pose",10);
    rospub_pose8 = nodeHandle.advertise<geometry_msgs::PoseStamped>("/drone8/local_position/pose",10);
    rospub_pose9 = nodeHandle.advertise<geometry_msgs::PoseStamped>("/drone9/local_position/pose",10);
    rospub_pose10 = nodeHandle.advertise<geometry_msgs::PoseStamped>("/drone10/local_position/pose",10);
    rospub_pose11 = nodeHandle.advertise<geometry_msgs::PoseStamped>("/drone11/local_position/pose",10);
    rospub_pose12 = nodeHandle.advertise<geometry_msgs::PoseStamped>("/drone12/local_position/pose",10);
    rospub_pose13 = nodeHandle.advertise<geometry_msgs::PoseStamped>("/drone13/local_position/pose",10);
    rospub_pose14 = nodeHandle.advertise<geometry_msgs::PoseStamped>("/drone14/local_position/pose",10);
    rospub_pose15 = nodeHandle.advertise<geometry_msgs::PoseStamped>("/drone15/local_position/pose",10);
    rospub_pose16 = nodeHandle.advertise<geometry_msgs::PoseStamped>("/drone16/local_position/pose",10);
    rospub_pose17 = nodeHandle.advertise<geometry_msgs::PoseStamped>("/drone17/local_position/pose",10);
    rospub_pose18 = nodeHandle.advertise<geometry_msgs::PoseStamped>("/drone18/local_position/pose",10);
    rospub_pose19 = nodeHandle.advertise<geometry_msgs::PoseStamped>("/drone19/local_position/pose",10);
    rospub_pose20 = nodeHandle.advertise<geometry_msgs::PoseStamped>("/drone20/local_position/pose",10);
    rospub_pose21 = nodeHandle.advertise<geometry_msgs::PoseStamped>("/drone21/local_position/pose",10);
    rospub_pose22 = nodeHandle.advertise<geometry_msgs::PoseStamped>("/drone22/local_position/pose",10);

    rossub_planner = nodeHandle.subscribe<visualization_msgs::Marker>("/planner_way", 10, planner_callback);
    rospub_planner = nodeHandle.advertise<visualization_msgs::Marker>("/planner_way_line",10);

    marker_pub = nodeHandle.advertise<visualization_msgs::Marker>("/zhou/target_position_box", 10);
    marker_id_pub = nodeHandle.advertise<visualization_msgs::Marker>("/zhou/target_name", 10);
    drone_id_pub = nodeHandle.advertise<visualization_msgs::Marker>("/zhou/drone_name", 10);

    rossub_pointcloud = nodeHandle.subscribe<sensor_msgs::PointCloud2>("/zhao/point_cloud", 10, pointcloud_callback);

    rospub_pointcloud = nodeHandle.advertise<sensor_msgs::PointCloud2>("/drone/pointcloud",10);

    rossub_target = nodeHandle.subscribe<geometry_msgs::PoseStamped>("/zhao/target", 10, target_callback);


    //循环回调
    while(ros::ok()&&nodeHandle.ok()){        
        ros::spinOnce();
        loop_rate.sleep();
    }

    

    nodeHandle.shutdown();
    return 0;
}

void goal_callback(const geometry_msgs::PoseStamped::ConstPtr &goal_msg)
{
    geometry_msgs::PoseStamped current_pose;
    current_pose.header.seq = goal_msg->header.seq;
    current_pose.header.stamp = ros::Time::now();
    current_pose.header.frame_id = "map";
    current_pose.pose.position.x = goal_msg->pose.position.x;
    current_pose.pose.position.y = goal_msg->pose.position.y;
    current_pose.pose.position.z = goal_msg->pose.position.z;
    current_pose.pose.orientation.x = goal_msg->pose.orientation.x;
    current_pose.pose.orientation.y = goal_msg->pose.orientation.y;
    current_pose.pose.orientation.z = goal_msg->pose.orientation.z;
    current_pose.pose.orientation.w = goal_msg->pose.orientation.w;
    std::string str = goal_msg->header.frame_id;
    int drone_id = atoi(str.c_str());
    
    if(drone_id <=12){
        geometry_msgs::PoseStamped all_pose;
        all_pose = *goal_msg;
        rospub_poseall.publish(all_pose);
    }
        

    drone_pose_vector[drone_id-1].pose.position.x = current_pose.pose.position.x;
    drone_pose_vector[drone_id-1].pose.position.y = current_pose.pose.position.y;
    drone_pose_vector[drone_id-1].pose.position.z = current_pose.pose.position.z+0.5;
    std::string drone_name = "drone_"+str;
    draw_drone_name(drone_pose_vector[drone_id-1],drone_name, drone_id);

    switch (drone_id)
    {
    case 1:
        rospub_pose1.publish(current_pose);
        break;
    case 2:
        rospub_pose2.publish(current_pose);
        break;
    case 3:
        rospub_pose3.publish(current_pose);
        break;
    case 4:
        rospub_pose4.publish(current_pose);
        break;
    case 5:
        rospub_pose5.publish(current_pose);
        break;
    case 6:
        rospub_pose6.publish(current_pose);
        break;
    case 7:
        rospub_pose7.publish(current_pose);
        break;
    case 8:
        rospub_pose8.publish(current_pose);
        break;
    case 9:
        rospub_pose9.publish(current_pose);
        break;
    case 10:
        rospub_pose10.publish(current_pose);
        break;
    case 11:
        rospub_pose11.publish(current_pose);
        break;
    case 12:
        rospub_pose12.publish(current_pose);
        break;
    case 13:
        rospub_pose13.publish(current_pose);
        break;
    case 14:
        rospub_pose14.publish(current_pose);
        break;
    case 15:
        rospub_pose15.publish(current_pose);
        break;
    case 16:
        rospub_pose16.publish(current_pose);
        break;
    case 17:
        rospub_pose17.publish(current_pose);
        break;
    case 18:
        rospub_pose18.publish(current_pose);
        break;
    case 19:
        rospub_pose19.publish(current_pose);
        break;
    case 20:
        rospub_pose20.publish(current_pose);
        break;
    case 21:
        rospub_pose21.publish(current_pose);
        break;
    case 22:
        rospub_pose22.publish(current_pose);
        break;
    default:
        break;
    }
}

void planner_callback(const visualization_msgs::Marker::ConstPtr &target_msg){
    std::vector<geometry_msgs::Pose> draw_poses_temp;
    for(size_t i = 0;i<target_msg->points.size();i++)
    {
        geometry_msgs::Pose pt;
        pt.position.x = (float) target_msg->points[i].x;
        pt.position.y = (float) target_msg->points[i].y;
        pt.position.z = (float) target_msg->points[i].z;
        draw_poses_temp.push_back(pt); 
    } 
    if(drone_id_cnt<1000)
    {
        drone_id_cnt++;
    }
    else
    {
        drone_id_cnt = 0;
    }
    draw_waypoints(draw_poses_temp, drone_id_cnt, 0.2);

}

void target_callback(const geometry_msgs::PoseStamped::ConstPtr &target_msg){
    geometry_msgs::PoseStamped center_pose;
    center_pose.header.frame_id = target_msg->header.frame_id;;
    center_pose.pose.position.x = target_msg->pose.position.x;
    center_pose.pose.position.y = target_msg->pose.position.y;
    center_pose.pose.position.z = target_msg->pose.position.z;
    if (target_msg->header.frame_id == "human") {
        int have_near_target = 0;
        for (int j = 0; j < human_list.size(); j++) {
            double dist_two_target = sqrt((center_pose.pose.position.x - human_list[j].pose.position.x) *
                                          (center_pose.pose.position.x - human_list[j].pose.position.x) +
                                          (center_pose.pose.position.y - human_list[j].pose.position.y) *
                                          (center_pose.pose.position.y - human_list[j].pose.position.y) +
                                          (center_pose.pose.position.z - human_list[j].pose.position.z) *
                                          (center_pose.pose.position.z - human_list[j].pose.position.z));
            if (dist_two_target < 1.0) {
                human_list[j] = center_pose;
                have_near_target++;
                break;
            }
        }
        if (have_near_target == 0) {
            human_list.push_back(center_pose);
        }
    } else if (target_msg->header.frame_id == "car") {
        int have_near_target = 0;
        for (int j = 0; j < car_list.size(); j++) {
            double dist_two_target = sqrt((center_pose.pose.position.x - car_list[j].pose.position.x) *
                                          (center_pose.pose.position.x - car_list[j].pose.position.x) +
                                          (center_pose.pose.position.y - car_list[j].pose.position.y) *
                                          (center_pose.pose.position.y - car_list[j].pose.position.y) +
                                          (center_pose.pose.position.z - car_list[j].pose.position.z) *
                                          (center_pose.pose.position.z - car_list[j].pose.position.z));
            if (dist_two_target < 1.0) {
                car_list[j] = center_pose;
                have_near_target++;
                break;
            }
        }
        if (have_near_target == 0) {
            car_list.push_back(center_pose);
        }
    } else if (target_msg->header.frame_id == "oil") {
        int have_near_target = 0;
        for (int j = 0; j < oil_list.size(); j++) {
            double dist_two_target = sqrt((center_pose.pose.position.x - oil_list[j].pose.position.x) *
                                          (center_pose.pose.position.x - oil_list[j].pose.position.x) +
                                          (center_pose.pose.position.y - oil_list[j].pose.position.y) *
                                          (center_pose.pose.position.y - oil_list[j].pose.position.y) +
                                          (center_pose.pose.position.z - oil_list[j].pose.position.z) *
                                          (center_pose.pose.position.z - oil_list[j].pose.position.z));
            if (dist_two_target < 1.0) {
                oil_list[j] = center_pose;
                have_near_target++;
                break;
            }
        }
        if (have_near_target == 0) {
            oil_list.push_back(center_pose);
        }
    } else if (target_msg->header.frame_id == "weapon") {
        int have_near_target = 0;
        for (int j = 0; j < weapon_list.size(); j++) {
            double dist_two_target = sqrt((center_pose.pose.position.x - weapon_list[j].pose.position.x) *
                                          (center_pose.pose.position.x - weapon_list[j].pose.position.x) +
                                          (center_pose.pose.position.y - weapon_list[j].pose.position.y) *
                                          (center_pose.pose.position.y - weapon_list[j].pose.position.y) +
                                          (center_pose.pose.position.z - weapon_list[j].pose.position.z) *
                                          (center_pose.pose.position.z - weapon_list[j].pose.position.z));
            if (dist_two_target < 1.0) {
                weapon_list[j] = center_pose;
                have_near_target++;
                break;
            }
        }
        if (have_near_target == 0) {
            weapon_list.push_back(center_pose);
        }
    } else if (target_msg->header.frame_id == "bigarmor") {
        int have_near_target = 0;
        for (int j = 0; j < bigarmor_list.size(); j++) {
            double dist_two_target = sqrt((center_pose.pose.position.x - bigarmor_list[j].pose.position.x) *
                                          (center_pose.pose.position.x - bigarmor_list[j].pose.position.x) +
                                          (center_pose.pose.position.y - bigarmor_list[j].pose.position.y) *
                                          (center_pose.pose.position.y - bigarmor_list[j].pose.position.y) +
                                          (center_pose.pose.position.z - bigarmor_list[j].pose.position.z) *
                                          (center_pose.pose.position.z - bigarmor_list[j].pose.position.z));
            if (dist_two_target < 1.0) {
                bigarmor_list[j] = center_pose;
                have_near_target++;
                break;
            }
        }
        if (have_near_target == 0) {
            bigarmor_list.push_back(center_pose);
        }
    } else if (target_msg->header.frame_id == "shatanche") {
        int have_near_target = 0;
        for (int j = 0; j < shatanche_list.size(); j++) {
            double dist_two_target = sqrt((center_pose.pose.position.x - shatanche_list[j].pose.position.x) *
                                          (center_pose.pose.position.x - shatanche_list[j].pose.position.x) +
                                          (center_pose.pose.position.y - shatanche_list[j].pose.position.y) *
                                          (center_pose.pose.position.y - shatanche_list[j].pose.position.y) +
                                          (center_pose.pose.position.z - shatanche_list[j].pose.position.z) *
                                          (center_pose.pose.position.z - shatanche_list[j].pose.position.z));
            if (dist_two_target < 1.0) {
                shatanche_list[j] = center_pose;
                have_near_target++;
                break;
            }
        }
        if (have_near_target == 0) {
            shatanche_list.push_back(center_pose);
        }
    }
}

void pointcloud_callback(const sensor_msgs::PointCloud2::ConstPtr &drone_cloud){
    pcl::PointCloud<pcl::PointXYZ> input_cloud;
    pcl::fromROSMsg(*drone_cloud, input_cloud);
  
    // 2. 过滤 Z 超出范围的点
    pcl::PointCloud<pcl::PointXYZ> prefiltered;
    prefiltered.reserve(input_cloud.size());
    for (auto& pt : input_cloud) {
      if (std::abs(pt.z) <= 10.0) {
        prefiltered.push_back(pt);
      }
    }
  
    // 3. 合并到 cloud_fusion
    cloud_fusion += prefiltered;  
    cloud_fusion.width  = cloud_fusion.points.size();
    cloud_fusion.height = 1;
    // std::cout << "Before conversion: size=" << cloud_fusion.size() 
    //       << ", width=" << cloud_fusion.width 
    //       << ", height=" << cloud_fusion.height << std::endl;
    if (cloud_fusion.points.size() != cloud_fusion.width * cloud_fusion.height) {
        std::cout<<"====>error points111.."<<std::endl;
    }
    
    //sample pointcloud
    pcl::PCLPointCloud2::Ptr cloud_in (new pcl::PCLPointCloud2 ());
    pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ());
    toPCLPointCloud2(cloud_fusion,*cloud_in);
    pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
    sor.setInputCloud (cloud_in);
    sor.setLeafSize(sample_size,sample_size,sample_size);
    sor.filter (*cloud_filtered);
    fromPCLPointCloud2(*cloud_filtered,cloud_fusion);
    cloud_fusion.width = cloud_fusion.points.size();
    cloud_fusion.height = 1;
}

void draw_box(Box temp_box, int id, double width, double red, double green, double blue)
{
    //draw box on rviz
    visualization_msgs::Marker line_strip;
    line_strip.header.frame_id = "map";
    line_strip.header.stamp = ros::Time::now();
    line_strip.ns = "target_box";//namespace
    line_strip.action = visualization_msgs::Marker::ADD;
    line_strip.pose.orientation.w = 1.0;
    line_strip.id = id; //unique id, useful when multiple markers exist.
    line_strip.type = visualization_msgs::Marker::LINE_STRIP; //marker type
    line_strip.lifetime = ros::Duration(5);
    line_strip.scale.x = width; //width of the line
    line_strip.color.r = red; 
    line_strip.color.g = green; 
    line_strip.color.b = blue;
    line_strip.color.a = 1.0; //alpha ,set 1
    geometry_msgs::Point p[8];
    p[0].x = temp_box.x_min;     p[0].y = temp_box.y_max;     p[0].z = temp_box.z_max;
    p[1].x = temp_box.x_min;     p[1].y = temp_box.y_min;     p[1].z = temp_box.z_max;
    p[2].x = temp_box.x_min;     p[2].y = temp_box.y_min;     p[2].z = temp_box.z_min;
    p[3].x = temp_box.x_min;     p[3].y = temp_box.y_max;     p[3].z = temp_box.z_min;
    p[4].x = temp_box.x_max;     p[4].y = temp_box.y_max;     p[4].z = temp_box.z_min;
    p[5].x = temp_box.x_max;     p[5].y = temp_box.y_min;     p[5].z = temp_box.z_min;
    p[6].x = temp_box.x_max;     p[6].y = temp_box.y_min;     p[6].z = temp_box.z_max;
    p[7].x = temp_box.x_max;     p[7].y = temp_box.y_max;     p[7].z = temp_box.z_max;
    //LINE_STRIP link 0-1 1-2 2-3 only
    for (int i = 0; i < 8; i++){
        line_strip.points.push_back(p[i]);
    }
    //so we need to add every links mannualy
    line_strip.points.push_back(p[0]);
    line_strip.points.push_back(p[3]);
    line_strip.points.push_back(p[2]);
    line_strip.points.push_back(p[5]);
    line_strip.points.push_back(p[6]);
    line_strip.points.push_back(p[1]);
    line_strip.points.push_back(p[0]);
    line_strip.points.push_back(p[7]);
    line_strip.points.push_back(p[4]);
    marker_pub.publish(line_strip);
    
    //end draw   
}
void draw_name(geometry_msgs::PoseStamped draw_pose,std::string name, int id)
{
    //draw target name 
    visualization_msgs::Marker marker_id;
    marker_id.header.frame_id="map";
    marker_id.header.stamp = ros::Time::now();
    marker_id.ns = "chuzhi";
    marker_id.action = visualization_msgs::Marker::ADD;
    marker_id.pose.orientation.w = 1.0;
    marker_id.id =id;
    marker_id.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
    marker_id.lifetime = ros::Duration(5);
    marker_id.scale.z = 1.0;
    marker_id.color.b = 0;
    marker_id.color.g = 0;
    marker_id.color.r = 255;
    marker_id.color.a = 1;
    geometry_msgs::Pose name_pose;
    name_pose.position.x =  draw_pose.pose.position.x;
    name_pose.position.y =  draw_pose.pose.position.y;
    name_pose.position.z =  draw_pose.pose.position.z+1.0;
    marker_id.text= name;
    marker_id.pose= name_pose;
    marker_id_pub.publish(marker_id);
}

void draw_drone_name(geometry_msgs::PoseStamped draw_pose,std::string name, int id)
{
    //draw target name 
    visualization_msgs::Marker marker_id;
    marker_id.header.frame_id="map";
    marker_id.header.stamp = ros::Time::now();
    marker_id.ns = "drone_name";
    marker_id.action = visualization_msgs::Marker::ADD;
    marker_id.pose.orientation.w = 1.0;
    marker_id.id =id;
    marker_id.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
    marker_id.lifetime = ros::Duration(1);
    marker_id.scale.z = 1.0;
    marker_id.color.b = 0;
    marker_id.color.g = 0;
    marker_id.color.r = 255;
    marker_id.color.a = 1;
    geometry_msgs::Pose name_pose;
    name_pose.position.x =  draw_pose.pose.position.x;
    name_pose.position.y =  draw_pose.pose.position.y;
    name_pose.position.z =  draw_pose.pose.position.z+1.0;
    marker_id.text= name;
    marker_id.pose= name_pose;
    drone_id_pub.publish(marker_id);
}

void draw_waypoints(std::vector<geometry_msgs::Pose> draw_poses, int id,double width)
{
    //draw target name 
    visualization_msgs::Marker marker_way;
    marker_way.header.frame_id="map";
    marker_way.header.stamp = ros::Time::now();
    marker_way.ns = "waypoints";
    marker_way.action = visualization_msgs::Marker::ADD;
    marker_way.pose.orientation.w = 1.0;
    marker_way.id =id;
    marker_way.type = visualization_msgs::Marker::LINE_STRIP;
    marker_way.lifetime = ros::Duration(10);
    marker_way.scale.x = width; //width of the line
    marker_way.color.b = 0;
    marker_way.color.g = 255;
    marker_way.color.r = 0;
    marker_way.color.a = 1;
    for (int i = 0; i < draw_poses.size(); i++){
        geometry_msgs::Point drawww;
        drawww.x = draw_poses[i].position.x;
        drawww.y = draw_poses[i].position.y;
        drawww.z = draw_poses[i].position.z;
        marker_way.points.push_back(drawww);
    }
    rospub_planner.publish(marker_way);
}